package uebung03_w;

import java.awt.BasicStroke;
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.MouseWheelEvent;
import java.awt.event.MouseWheelListener;
import java.awt.geom.AffineTransform;
import java.awt.geom.NoninvertibleTransformException;
import java.awt.geom.Point2D;
import java.awt.image.BufferedImage;
import java.util.ArrayList;

import javax.imageio.ImageIO;
import javax.swing.JComponent;
import javax.swing.JFrame;

import org.ejml.simple.SimpleEVD;
import org.ejml.simple.SimpleMatrix;

import tools.Arrow;
import tools.ColorConversions;
import tools.Derivative;
import tools.Gauss;
import tools.Image;

public class KLT04 {
	public static void main(String[] args) throws Exception {
		
		final BufferedImage imgI1 = ImageIO.read(KLT04.class.getClassLoader().getResource("res/lena1.png"));
		final BufferedImage imgI2 = ImageIO.read(KLT04.class.getClassLoader().getResource("res/lena2.png"));
		
		final int w = imgI1.getWidth(), h = imgI1.getHeight();
		
		int[] intlumI1 = ColorConversions.getFastLuminance(imgI1.getRGB(0, 0, w, h, new int[w*h], 0, w), new int[w*h]);
		int[] intlumI2 = ColorConversions.getFastLuminance(imgI2.getRGB(0, 0, w, h, new int[w*h], 0, w), new int[w*h]);
		
		int[] pixels = ColorConversions.buildRGB(intlumI1, new int[w*h], intlumI2, new int[w*h]);
		imgI1.setRGB(0, 0, w, h, pixels, 0, w);
		
		double [] lumI1 = Image.getScalarLuminance(intlumI1, new double[w*h]);
		double [] lumI2 = Image.getScalarLuminance(intlumI2, new double[w*h]);

		double[] dxI1 = Derivative.dx(lumI1, w, h, new double[w*h]);
		double[] dyI1 = Derivative.dy(lumI1, w, h, new double[w*h]);
		
		double[] dxI2 = Derivative.dx(lumI2, w, h, new double[w*h]);
		double[] dyI2 = Derivative.dy(lumI2, w, h, new double[w*h]);
		
		double[] dtI = Derivative.dt(lumI1, lumI2, new double[w*h]);
		
		final int step = 5, neighbours = 5;
		final ArrayList<Point2D> uv = new ArrayList<Point2D>(), xy = new ArrayList<Point2D>(), eigenValues = new ArrayList<Point2D>();
		
		SimpleMatrix sm = new SimpleMatrix(2, 2);
		
		for (int y = neighbours; y<h-neighbours; y+=step) {
			for (int x = neighbours; x<w-neighbours; x+=step) {
				
				double u = 0, v = 0;
				double hx = 1, hy = 1;
				
				double E1 = 0, E2 = 0, E5 = 0, E3 = 0, E6 = 0, sumW = 0;
				
				for (int j = -neighbours; j<neighbours; j++) {
					for (int i = -neighbours; i<neighbours; i++) {
						int index = i+x+(y+j)*w;
						double dx1 = dxI1[index], dy1 = dyI1[index], dx2 = dxI2[index], dy2 = dyI2[index], dt = dtI[index];
						
//						double dx = Image.bilerp(dxI2, x+u, y+u, w, h), dy = Image.bilerp(dyI2, x+u, y+v, w, h), dt = Image.bilerp(dtI, x+u, y+u, w, h);
						
						double weight = Gauss.gaussian1d(i*i+j*j, neighbours/3.); 
						
//						weight = 1;
//						if (Double.compare(dt-dx-dy, 0)!=0)
//							System.out.println("dx: "+dx+" dy: "+dy+" dt: "+dt);
						
						E1 += (dx1*dx1)*weight; 
						E2 += (dy1*dx1)*weight;
						E5 += (dy1*dy1)*weight;
						
						E3 -= (dt*(dx1))*weight;
						E6 -= (dt*(dy1))*weight;
						
						sumW += weight;
					}
				}
				
				E1 /= sumW;
				E2 /= sumW;
				E5 /= sumW;
				
				E3 /= sumW;
				E6 /= sumW;
				
				System.out.println(E1+" "+E2+" "+E3+" "+E5+" "+E6);
				
				double ll = (E2*E2-E1*E5);
				
				sm.set(0, 0, E1);
				sm.set(1, 0, E2);
				sm.set(0, 1, E2);
				sm.set(1, 1, E5);
				
				if (Double.compare(0, ll)!=0) {
					if (Double.compare(sm.determinant(), 0)!=0) {
						SimpleEVD<SimpleMatrix> evd = sm.eig();
						
						eigenValues.add(new Point2D.Double(evd.getEigenvalue(0).real, evd.getEigenvalue(1).real));
						
						hx = (E5*E3-E2*E6)/ll;
						hy = (E2*E3-E1*E6)/ll;
						
						u += hx;
						v += hy;

						xy.add(new Point2D.Double(x,y));
						uv.add(new Point2D.Double(u,v));
						System.out.println(uv.get(uv.size()-1));

					}
				}
				
			}
		}
		
		double max = 0;
		for (int i = 0; i<eigenValues.size(); i++) {
			Point2D p = eigenValues.get(i);
			max = Math.max(max, Math.max(p.getX(), p.getY()));
		}
		final double limit = max*0.1;
		
		System.out.println(uv.size());
		
		JFrame ff = new JFrame();
		ff.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		final AffineTransform zoom = AffineTransform.getScaleInstance(2, 2);
		
		final JComponent canvas = new JComponent() {
			{setPreferredSize(new Dimension(w*2,h*2));}
			Arrow arrw = new Arrow();
			@Override
			public void paint(Graphics g) {
				Graphics2D g2 = (Graphics2D)g;
				g2.transform(zoom);
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
				g2.drawImage(imgI1, 0, 0, this);
				g2.setStroke(new BasicStroke(0.1f));
				
				for (int i = 0; i<uv.size(); i++) {
					Point2D eig = eigenValues.get(i);
					if (Math.max(eig.getX(), eig.getY())>limit) {
						Point2D p1 = xy.get(i), p2 = uv.get(i);
						Point2D dst = new Point2D.Double(p1.getX()+p2.getX(), p1.getY()+p2.getY());
						
						arrw.setArrow(p1, dst, 1, 5);
						
						g2.setColor(Color.magenta);
						g2.fill(arrw);
						g2.setColor(Color.green);
						g2.draw(arrw);
						
					}
				}
				super.paint(g);
			}
		};
		canvas.addMouseWheelListener(new MouseWheelListener() {
				
				@Override
				public void mouseWheelMoved(MouseWheelEvent e) {
					
					double scale = Math.pow(1.01, -e.getWheelRotation());

					try {
						Point2D p = new Point2D.Double(e.getX(), e.getY()); 
						zoom.inverseTransform(p, p);
						zoom.concatenate(AffineTransform.getTranslateInstance(p.getX(), p.getY()));
						zoom.concatenate(AffineTransform.getScaleInstance(scale, scale));
						zoom.concatenate(AffineTransform.getTranslateInstance(-p.getX(), -p.getY()));
					}
					catch (NoninvertibleTransformException nte ) {
						nte.printStackTrace();
					}
					canvas.repaint();
				}
			});
		
		//Garland Hackberd:
		
		final ArrayList<Point2D> normals = new ArrayList<Point2D>();
		final ArrayList<Double> distances = new ArrayList<Double>();
		
		for (int i = 0; i<uv.size(); i++) {
			Point2D eig = eigenValues.get(i);
			if (Math.max(eig.getX(), eig.getY())>limit) {
				Point2D a = xy.get(i), b = uv.get(i);
				double norml = Math.sqrt(b.getX()*b.getX()+b.getY()*b.getY());
				Point2D normal = new Point2D.Double(b.getY()/norml, -b.getX()/norml);
				normals.add(normal);
				distances.add(a.getX()*normal.getX()+a.getY()*normal.getY());
			}
		}
		
		double xx = 0, yx = 0, yy = 0, tx = 0, ty = 0;
		
		for (int i = 0; i<normals.size(); i++) {
			Point2D norml = normals.get(i);
			double a1 = norml.getX(), a2 = norml.getY(), c = distances.get(i);
			xx += a1*a1;
			yx += a1*a2;
			yy += a2*a2;
			tx += a1*c;
			ty += a2*c;
		}

		sm.set(0, 0, xx);
		sm.set(0, 1, yx);
		sm.set(1, 0, yx);
		sm.set(1, 1, yy);
		
		SimpleMatrix avec = new SimpleMatrix(2, 1);
		avec.set(0, 0, tx);
		avec.set(1, 0, ty);
		
		SimpleMatrix vanish = sm.solve(avec);
		System.out.println(vanish.get(0, 0)+" "+vanish.get(1, 0));
		
		ff.add(canvas, BorderLayout.CENTER);
		
		ff.pack();
		ff.setVisible(true);
		
	}
}
